/**
  ****************************(C) COPYRIGHT 2024 Polarbear****************************
  * @file       shoot_fric.c/h
  * @brief      使用摩擦轮的发射机构控制器。
  * @note       包括初始化，目标量更新、状态量更新、控制量计算与直接控制量的发送
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Apr-1-2024      Penguin         1. done
  *  V1.0.1     Apr-16-2024     Penguin         1. 完成基本框架
  *  V1.1.0     2025-1-15       CJH             1. 实现基本功能
  *  V2.0.0     2025-3-3        CJH             1. 兼容了达妙4310拨弹盘和大疆2006拨弹盘
  *                                             2. 完善了单发功能，上位机火控功能
  *                                             3. 增加了热量限制
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2024 Polarbear****************************
*/

#include "shoot_fric_trigger.h"
#include "stm32f4xx_hal.h"


#if (SHOOT_TYPE == SHOOT_FRIC_TRIGGER)

static Shoot_s SHOOT = {
  .mode = LOAD_STOP,
  .state = FRIC_NOT_READY,
  .fric_flag = 0,
  .move_flag = 0,
  .ecd_count = 0,
  .shoot_flag = 0,
  .heat = 0,
  .heat_limit = 0,

};

//static uint32_t last_shoot_time = 0; // 记录上次单发时间(ms)

uint8_t fric_ui;
fp32 delta;

/*-------------------- Init --------------------*/

/**
 * @brief          初始化
 * @param[in]      none
 * @retval         none
 */
void ShootInit(void) 
{ 
  //获取遥控器指针
  SHOOT.rc = get_remote_control_point(); 

  //摩擦轮相关
  MotorInit(&SHOOT.fric_motor[0],FRIC_MOTOR_R_ID, FRIC_MOTOR_R_CAN, FRIC_MOTOR_TYPE, 1, 1.0f, 0);//初始化R摩擦轮电机结构体
  MotorInit(&SHOOT.fric_motor[1],FRIC_MOTOR_L_ID, FRIC_MOTOR_L_CAN, FRIC_MOTOR_TYPE, 1, 1.0f, 0);//初始化L摩擦轮电机结构体
  MotorInit(&SHOOT.fric_motor[2],FRIC_MOTOR_R1_ID, FRIC_MOTOR_R1_CAN, FRIC_MOTOR_TYPE, 1, 1.0f, 0);//初始化R1摩擦轮电机结构体
  MotorInit(&SHOOT.fric_motor[3],FRIC_MOTOR_L1_ID, FRIC_MOTOR_L1_CAN, FRIC_MOTOR_TYPE, 1, 1.0f, 0);//初始化L1摩擦轮电机结构体

  const fp32 pid_fric[3] = {FRIC_SPEED_PID_KP, FIRC_SPEED_PID_KI, FRIC_SPEED_PID_KD};//摩擦轮速度环

  PID_init(&SHOOT.fric_pid[0], PID_POSITION, pid_fric, FRIC_PID_MAX_OUT, FRIC_PID_MAX_IOUT);
  PID_init(&SHOOT.fric_pid[1], PID_POSITION, pid_fric, FRIC_PID_MAX_OUT, FRIC_PID_MAX_IOUT);//摩擦轮初始化pid
  PID_init(&SHOOT.fric_pid[2], PID_POSITION, pid_fric, FRIC_PID_MAX_OUT, FRIC_PID_MAX_IOUT);
  PID_init(&SHOOT.fric_pid[3], PID_POSITION, pid_fric, FRIC_PID_MAX_OUT, FRIC_PID_MAX_IOUT);

  //拨弹盘相关
  MotorInit(&SHOOT.trigger_motor,TRIGGER_MOTOR_ID, TRIGGER_MOTOR_CAN, TRIGGER_MOTOR_TYPE, 1, 1.0f, 0);//初始化拨弹盘电机结构体
 if (TRIGGER_MOTOR_TYPE == DJI_M2006)
 {
  const fp32 pid_angel_trigger[3] = {TRIGGER_ANGEL_PID_KP, TRIGGER_ANGEL_PID_KI, TRIGGER_ANGEL_PID_KD};//拨弹盘角度环
  const fp32 pid_speed_trigger[3] = {TRIGGER_SPEED_PID_KP, TRIGGER_SPEED_PID_KI, TRIGGER_SPEED_PID_KD};//拨弹盘速度环

  PID_init(&SHOOT.trigger_angel_pid, PID_POSITION, pid_angel_trigger, TRIGGER_ANGEL_PID_MAX_OUT, TRIGGER_ANGEL_PID_MAX_IOUT);
  PID_init(&SHOOT.trigger_speed_pid, PID_POSITION, pid_speed_trigger, TRIGGER_SPEED_PID_MAX_OUT, TRIGGER_SPEED_PID_MAX_IOUT);  //拨弹盘初始化pid
 }
 else if (TRIGGER_MOTOR_TYPE == DM_4310)
 {
  const fp32 pid_angel_trigger[3] = {TRIGGER_ANGEL_PID_KP, TRIGGER_ANGEL_PID_KI, TRIGGER_ANGEL_PID_KD};//拨弹盘角度环

  PID_init(&SHOOT.trigger_angel_pid, PID_POSITION, pid_angel_trigger, TRIGGER_ANGEL_PID_MAX_OUT, TRIGGER_ANGEL_PID_MAX_IOUT); //拨弹盘初始化pid
 }
 
}

/*-------------------- Set mode --------------------*/

/**
 * @brief          设置模式
 * @param[in]      none
 * @retval         none
 */
void ShootSetMode(void)
{
  /*键鼠遥控器控制方式初版----------------------------*/
{
  if (switch_is_up(SHOOT.rc->rc.s[SHOOT_MODE_CHANNEL]))
  {
       //SHOOT.state = FRIC_READY;
       SHOOT.mode = LOAD_DART;
  } 

  else if (switch_is_mid(SHOOT.rc->rc.s[SHOOT_MODE_CHANNEL]))
  {
       //SHOOT.state = FRIC_READY;
       SHOOT.mode = LOAD_DART;
       
    

   } 
   else if (switch_is_down(SHOOT.rc->rc.s[SHOOT_MODE_CHANNEL]))
   {
       //SHOOT.state = FRIC_NOT_READY;
       SHOOT.mode = LOAD_STOP;


   }

}

{
  if (switch_is_up(SHOOT.rc->rc.s[0]))
  {
       SHOOT.state = FRIC_NOT_READY;
       //SHOOT.mode = LOAD_START;
  } 
  
  else if (switch_is_mid(SHOOT.rc->rc.s[0]))
  {
       SHOOT.state = FRIC_READY;
       //SHOOT.mode = LOAD_DART;
       
    
  
   } 
   else if (switch_is_down(SHOOT.rc->rc.s[0]))
   {
       SHOOT.state = FRIC_NOT_READY;
       //SHOOT.mode = LOAD_STOP;
  
  
   }  
}
    

   
    //安全档
  
        if ((switch_is_down(SHOOT.rc->rc.s[0])))
    {
      SHOOT.mode = LOAD_STOP;
      SHOOT.state = FRIC_NOT_READY;
    }
    

    //遥控器离线保护
    if ( toe_is_error(DBUS_TOE) )
    {        
      SHOOT.state = FRIC_NOT_READY;
      SHOOT.mode = LOAD_STOP;
    }
}

/*-------------------- Observe --------------------*/

/**
 * @brief          更新状态量
 * @param[in]      none
 * @retval         none
 */
void ShootObserver(void) 
{
  GetMotorMeasure(&SHOOT.trigger_motor);
  GetMotorMeasure(&SHOOT.fric_motor[0]);
  GetMotorMeasure(&SHOOT.fric_motor[1]);
  GetMotorMeasure(&SHOOT.fric_motor[2]);
  GetMotorMeasure(&SHOOT.fric_motor[3]);

  SHOOT.FDB.fric_speed_fdb_R = SHOOT.fric_motor[0].fdb.vel;
  SHOOT.FDB.fric_speed_fdb_L = SHOOT.fric_motor[1].fdb.vel;
  SHOOT.FDB.fric_speed_fdb_R1 = SHOOT.fric_motor[2].fdb.vel;
  SHOOT.FDB.fric_speed_fdb_L1 = SHOOT.fric_motor[3].fdb.vel;

  SHOOT.FDB.trigger_speed_fdb = SHOOT.trigger_motor.fdb.vel;

  if (TRIGGER_MOTOR_TYPE == DJI_M2006)
  {
    if (SHOOT.trigger_motor.fdb.ecd - SHOOT.last_ecd > HALF_ECD_RANGE)
    {
        SHOOT.ecd_count--;
    }
    else if (SHOOT.trigger_motor.fdb.ecd - SHOOT.last_ecd < -HALF_ECD_RANGE)
    {
        
        SHOOT.ecd_count++;
    }

    if (SHOOT.ecd_count == FULL_COUNT)
    {
        SHOOT.ecd_count = -(FULL_COUNT - 1);
    }
    else if (SHOOT.ecd_count == -FULL_COUNT)
    {
        SHOOT.ecd_count = FULL_COUNT-1;
    }
    //计算输出轴角度
    SHOOT.FDB.trigger_angel_fdb = (SHOOT.ecd_count * ECD_RANGE + SHOOT.trigger_motor.fdb.ecd )* MOTOR_ECD_TO_ANGLE;

    //记录上一个ecd值
   SHOOT.last_ecd = SHOOT.trigger_motor.fdb.ecd;

  }
  else if (TRIGGER_MOTOR_TYPE == DM_4310)
  {
    SHOOT.FDB.trigger_angel_fdb = theta_format(SHOOT.trigger_motor.fdb.pos);
  }
  
    //记录上一个拨弹盘vel,用于堵转模式判断
  SHOOT.last_trigger_vel = SHOOT.trigger_motor.fdb.vel;

    //记录上一个摩擦轮vel,用于过热保护
  SHOOT.last_fric_vel = SHOOT.fric_motor[0].fdb.vel;
}

/*-------------------- Reference --------------------*/

/**
 * @brief          更新目标量
 * @param[in]      none
 * @retval         none
 */
void ShootReference(void) 
{
//	  float move4_put;
	
  switch (SHOOT.state)
  {
  case FRIC_NOT_READY:
  pump_pwm_set(500, 1);
    SHOOT.REF.fric_speed_ref_R=0.0f;
    SHOOT.REF.fric_speed_ref_L=0.0f;
    SHOOT.REF.fric_speed_ref_R1=0.0f;
    SHOOT.REF.fric_speed_ref_L1=0.0f;
    break;

  case FRIC_READY:
  pump_pwm_set(2500, 1);
   SHOOT.REF.fric_speed_ref_R=0.0f;
   SHOOT.REF.fric_speed_ref_L=0.0f;
   SHOOT.REF.fric_speed_ref_R1=0.0f;
   SHOOT.REF.fric_speed_ref_L1=0.0f;
    break;
  
  default:
  break;
  }

  switch (SHOOT.mode)
  {
  case LOAD_STOP:
  SHOOT.REF.trigger_speed_ref=0.0f;

  break;

  case LOAD_START:{
	  //SHOOT.REF.trigger_speed_ref=0.0f;
//上下
  fp32 move4_input = fp32_constrain(SHOOT.rc->rc.ch[1], REMOTE_CONTROLLER_MIN_DEADLINE, REMOTE_CONTROLLER_MAX_DEADLINE);


if (move4_input<-3){
        SHOOT.REF.fric_speed_ref_R=FRIC_L1_SPEED;
        SHOOT.REF.fric_speed_ref_L=FRIC_L1_SPEED;
      }
      else if (move4_input>3){
        SHOOT.REF.fric_speed_ref_R=-FRIC_L1_SPEED;
        SHOOT.REF.fric_speed_ref_L=-FRIC_L1_SPEED;
      }
      else if (move4_input<3&&move4_input>-3){
        SHOOT.REF.fric_speed_ref_R=0.0f;
        SHOOT.REF.fric_speed_ref_L=0.0f;
    }

			fp32 move3_input = fp32_constrain(SHOOT.rc->rc.ch[0], REMOTE_CONTROLLER_MIN_DEADLINE, REMOTE_CONTROLLER_MAX_DEADLINE);
//左右
      if (move3_input<-3){
        SHOOT.REF.fric_speed_ref_R1=FRIC_L2_SPEED;
      }
      else if (move3_input>3){
        SHOOT.REF.fric_speed_ref_R1=-40.0f;
      }
      else if (move3_input<=3&&move3_input>=-3){
        SHOOT.REF.fric_speed_ref_R1=0.0f;
    }
/*
fp32 move2_input = fp32_deadline(SHOOT.rc->rc.ch[2], REMOTE_CONTROLLER_MIN_DEADLINE, REMOTE_CONTROLLER_MAX_DEADLINE);
//皮筋调节
      if (move2_input<-3){
        SHOOT.REF.fric_speed_ref_R1=FRIC_R_SPEED;
      }
      else if (move2_input>3){
        SHOOT.REF.fric_speed_ref_R1=FRIC_L_SPEED;
      }
      else if (move2_input<=3&&move2_input>=-3){
        SHOOT.REF.fric_speed_ref_R1=0.0f;
    }
*/			

fp32 move1_input = fp32_constrain(SHOOT.rc->rc.ch[3], REMOTE_CONTROLLER_MIN_DEADLINE, REMOTE_CONTROLLER_MAX_DEADLINE);
//拉皮筋
      if (move1_input<-3){
        SHOOT.REF.fric_speed_ref_L1=FRIC_R1_SPEED;
      }
      else if (move1_input>3){
        SHOOT.REF.fric_speed_ref_L1=-FRIC_R1_SPEED;
      }
      else if (move1_input<=3&&move1_input>=-3){
        SHOOT.REF.fric_speed_ref_L1=0.0f;
    }			
	}

  break;

  case LOAD_DART:{
	  //SHOOT.REF.trigger_speed_ref=0.0f;
    
//上下
  fp32 move4_input = fp32_constrain(SHOOT.rc->rc.ch[1], REMOTE_CONTROLLER_MIN_DEADLINE, REMOTE_CONTROLLER_MAX_DEADLINE);


if (move4_input<-3){
        SHOOT.REF.fric_speed_ref_R=FRIC_L1_SPEED;
        SHOOT.REF.fric_speed_ref_L=FRIC_L1_SPEED;
      }
      else if (move4_input>3){
        SHOOT.REF.fric_speed_ref_R=-FRIC_L1_SPEED;
        SHOOT.REF.fric_speed_ref_L=-FRIC_L1_SPEED;
      }
      else if (move4_input<3&&move4_input>-3){
        SHOOT.REF.fric_speed_ref_R=0.0f;
        SHOOT.REF.fric_speed_ref_L=0.0f;
    }

			fp32 move3_input = fp32_constrain(SHOOT.rc->rc.ch[0], REMOTE_CONTROLLER_MIN_DEADLINE, REMOTE_CONTROLLER_MAX_DEADLINE);
//左右
      if (move3_input<-3){
        SHOOT.REF.fric_speed_ref_R1=FRIC_L2_SPEED;
      }
      else if (move3_input>3){
        SHOOT.REF.fric_speed_ref_R1=-40.0f;
      }
      else if (move3_input<=3&&move3_input>=-3){
        SHOOT.REF.fric_speed_ref_R1=0.0f;
    }
/*
fp32 move2_input = fp32_deadline(SHOOT.rc->rc.ch[2], REMOTE_CONTROLLER_MIN_DEADLINE, REMOTE_CONTROLLER_MAX_DEADLINE);
//皮筋调节
      if (move2_input<-3){
        SHOOT.REF.fric_speed_ref_R1=FRIC_R_SPEED;
      }
      else if (move2_input>3){
        SHOOT.REF.fric_speed_ref_R1=FRIC_L_SPEED;
      }
      else if (move2_input<=3&&move2_input>=-3){
        SHOOT.REF.fric_speed_ref_R1=0.0f;
    }
*/			

fp32 move1_input = fp32_constrain(SHOOT.rc->rc.ch[3], REMOTE_CONTROLLER_MIN_DEADLINE, REMOTE_CONTROLLER_MAX_DEADLINE);
//拉皮筋
      if (move1_input<-3){
        SHOOT.REF.fric_speed_ref_L1=FRIC_R1_SPEED;
      }
      else if (move1_input>3){
        SHOOT.REF.fric_speed_ref_L1=-FRIC_R1_SPEED;
      }
      else if (move1_input<=3&&move1_input>=-3){
        SHOOT.REF.fric_speed_ref_L1=0.0f;
    }			
	}
		
  default:
    break;
 } 

}
/*-------------------- Console --------------------*/

/**
 * @brief          计算控制量
 * @param[in]      none
 * @retval         none
 */
void ShootConsole(void) 
{
  /*
  SHOOT.fric_motor[0].set.curr= PID_calc(&SHOOT.fric_pid[0], SHOOT.FDB.fric_speed_fdb_R,SHOOT.REF.fric_speed_ref_R);
  SHOOT.fric_motor[1].set.curr= PID_calc(&SHOOT.fric_pid[1], SHOOT.FDB.fric_speed_fdb_L,SHOOT.REF.fric_speed_ref_L);
  SHOOT.fric_motor[2].set.curr= PID_calc(&SHOOT.fric_pid[2], SHOOT.FDB.fric_speed_fdb_R1,SHOOT.REF.fric_speed_ref_R1);
  SHOOT.fric_motor[3].set.curr= PID_calc(&SHOOT.fric_pid[3], SHOOT.FDB.fric_speed_fdb_L1,SHOOT.REF.fric_speed_ref_L1);
  */


  if (TRIGGER_MOTOR_TYPE == DJI_M2006)
  {
    if (SHOOT.mode == LOAD_STOP)
    {
        //SHOOT.trigger_motor.set.curr = PID_calc(&SHOOT.trigger_speed_pid, SHOOT.FDB.trigger_speed_fdb, SHOOT.REF.trigger_speed_ref);
        SHOOT.trigger_motor.set.curr = 0.0f;
    }
    else if (SHOOT.mode == LOAD_DART)
    {
      SHOOT.fric_motor[0].set.curr= PID_calc(&SHOOT.fric_pid[0], SHOOT.FDB.fric_speed_fdb_R,SHOOT.REF.fric_speed_ref_R);
      SHOOT.fric_motor[1].set.curr= PID_calc(&SHOOT.fric_pid[1], SHOOT.FDB.fric_speed_fdb_L,SHOOT.REF.fric_speed_ref_L);
      SHOOT.fric_motor[2].set.curr= PID_calc(&SHOOT.fric_pid[2], SHOOT.FDB.fric_speed_fdb_R1,SHOOT.REF.fric_speed_ref_R1);
      SHOOT.fric_motor[3].set.curr= PID_calc(&SHOOT.fric_pid[3], SHOOT.FDB.fric_speed_fdb_L1,SHOOT.REF.fric_speed_ref_L1);
    }
  }
}

/*-------------------- Cmd --------------------*/

/**
 * @brief          发送控制量
 * @param[in]      none
 * @retval         none
 */

void ShootSendCmd(void) 
{
  if (TRIGGER_MOTOR_TYPE == DJI_M2006)
  {
    CanCmdDjiMotor(FRIC_MOTOR_R_CAN, STD_ID , SHOOT.fric_motor[0].set.curr,SHOOT.fric_motor[1].set.curr,SHOOT.fric_motor[2].set.curr, SHOOT.fric_motor[3].set.curr);
    CanCmdDjiMotor(TRIGGER_MOTOR_CAN, 0x1FF , SHOOT.trigger_motor.set.curr,0,0,0);
  }
  else if (TRIGGER_MOTOR_TYPE == DM_4310)
  {
    DmEnable(&SHOOT.trigger_motor);
    DmMitCtrlVelocity(&SHOOT.trigger_motor, TRIGGER_SPEED_MIT_KD);
    CanCmdDjiMotor(FRIC_MOTOR_R_CAN, STD_ID ,SHOOT.fric_motor[0].set.curr,SHOOT.fric_motor[1].set.curr,SHOOT.fric_motor[2].set.curr, SHOOT.fric_motor[3].set.curr);
  }
}

#endif  // SHOOT_TYPE == SHOOT_FRIC
